
#ifndef RVIZ_DIS_01
#define RVIZ_DIS_01
#include "ros/ros.h"
#include <visualization_msgs/MarkerArray.h>

#include <string>
#include <eigen3/Eigen/Eigen>

typedef Eigen::MatrixXd Mat;
typedef Eigen::VectorXd Vec;

struct colorSimp {
    double r;
    double g;
    double b;
    double a;
    colorSimp(double r0 = 0.5, double g0 = 0.6, double b0 = 0.5, double a0 = 1.0) : r(r0), g(g0), b(b0), a(a0) {}
    ~colorSimp() {}
};

struct rviz1DisSimp {
    ros::NodeHandle n;
    ros::Publisher pub;
    ros::Rate r;
    std::string p_name;
    std::string f_name;
    std::string ns;
    visualization_msgs::MarkerArray ma;
    int id0;

    rviz1DisSimp(ros::NodeHandle &nh, const std::string publisher_name,
                                      const std::string frame_name,
                                      const std::string name_space)
        : n(nh),
          r(30),
          p_name(publisher_name),
          f_name(frame_name),
          ns(name_space),
          id0(0)
    {
        pub = n.advertise<visualization_msgs::MarkerArray>(publisher_name, 10);

        // 至少有一个订阅者订阅了这个话题，才开始发布消息
        while (pub.getNumSubscribers() == 0) {
            r.sleep();
        }
    }

    virtual ~rviz1DisSimp() {}

    visualization_msgs::Marker get_default_marker(int id) {
        visualization_msgs::Marker mvm;
        mvm.header.frame_id = f_name;
        mvm.header.stamp = ros::Time::now();
        mvm.ns = ns;
        mvm.action = visualization_msgs::Marker::ADD;
        mvm.id = id;
        mvm.type = visualization_msgs::Marker::LINE_STRIP;
        mvm.pose.orientation.w = 1.0;
        mvm.scale.x = 0.2;
        mvm.color.r = mvm.color.g = mvm.color.b = mvm.color.a = 1.0;
        return mvm;
    }

    // 绘制多边形障碍物
    int add_convex_poly_flat(int id,
                             Mat &vertex,
                             double height,
                             colorSimp c = colorSimp())
    {
        visualization_msgs::Marker mpoly = get_default_marker(id);
        //TRIANGLE_LIST 表示将所有的三角形以列表的形式进行描述
        mpoly.type = visualization_msgs::Marker::TRIANGLE_LIST;
        mpoly.scale.x = mpoly.scale.y = mpoly.scale.z = 1.0;

        std_msgs::ColorRGBA c1;
        c1.r = c.r;
        c1.g = c.g;
        c1.b = c.b;
        c1.a = c.a;
        mpoly.points.clear();
        mpoly.colors.clear();

        int num = vertex.rows();
        // 添加顶部和底部曲面的显示
        geometry_msgs::Point pl;
        for (int k = 1; k <= num - 2; ++k) {
            pl.z = 0.0;
            pl.x = vertex(0, 0);
            pl.y = vertex(0, 1);
            mpoly.points.push_back(pl);

            pl.x = vertex(k, 0);
            pl.y = vertex(k, 1);
            mpoly.points.push_back(pl);

            pl.x = vertex(k + 1, 0);
            pl.y = vertex(k + 1, 1);
            mpoly.points.push_back(pl);
            mpoly.colors.push_back(c1);

            pl.z = height;
            pl.x = vertex(0, 0);
            pl.y = vertex(0, 1);
            mpoly.points.push_back(pl);
            pl.x = vertex(k, 0);
            pl.y = vertex(k, 1);
            mpoly.points.push_back(pl);
            pl.x = vertex(k + 1, 0);
            pl.y = vertex(k + 1, 1);
            mpoly.points.push_back(pl);
            mpoly.colors.push_back(c1);
        }

        std_msgs::ColorRGBA c2;
        c2.r = 1.0;
        c2.g = 0.0;
        c2.b = 0.0;
        c2.a = c.a;

        // 在侧面添加曲面的显示
        int j = num - 1;
        for (int k = 0; k <= num - 1; ++k) {
            pl.z = height;
            pl.x = vertex(k, 0);
            pl.y = vertex(k, 1);
            mpoly.points.push_back(pl); // 1-当前顶点上的点

            pl.z = 0.0;
            mpoly.points.push_back(pl); // 1-当前顶点下的点
            pl.x = vertex(j, 0);
            pl.y = vertex(j, 1);
            mpoly.points.push_back(pl); // 1-上一个顶点下的点
            mpoly.colors.push_back(c2); // 1-组合成三角形设置颜色

            mpoly.points.push_back(pl); // 2-上一个顶点下的点

            pl.z = height;
            mpoly.points.push_back(pl); // 2-上一个顶点上的点
            pl.x = vertex(k, 0);
            pl.y = vertex(k, 1);
            mpoly.points.push_back(pl); // 2-当前顶点上的点
            mpoly.colors.push_back(c2); // 2-组合成三角形设置颜色
            j = k;
        }
        ma.markers.push_back(mpoly);
        return 0;
    }

    // 绘制圆形障碍物
    int add_cylinder(int id,
                     double x0, double y0, double r0, double height,
                     colorSimp c = colorSimp())
    {
        visualization_msgs::Marker mcyl = get_default_marker(id);
        mcyl.type = visualization_msgs::Marker::CYLINDER;
        mcyl.color.r = c.r;
        mcyl.color.g = c.g;
        mcyl.color.b = c.b;
        mcyl.color.a = c.a;
        mcyl.pose.position.x = x0;
        mcyl.pose.position.y = y0;
        mcyl.scale.x = mcyl.scale.y = r0 * 2;   // 半径
        mcyl.scale.z = height;                  // 高度
        ma.markers.push_back(mcyl);
        return 0;
    }

    // 绘制点信息
    int add_point_sphere(int id,
                         Eigen::Vector2d point,
                         colorSimp c = colorSimp())
    {
        visualization_msgs::Marker mpoint = get_default_marker(id);
        mpoint.type = visualization_msgs::Marker::SPHERE;
        mpoint.pose.position.x = point.x();
        mpoint.pose.position.y = point.y();
        mpoint.pose.position.z = 0.0;

        mpoint.scale.x = 0.3;
        mpoint.scale.y = 0.3;
        mpoint.scale.z = 0.2;
        mpoint.color.r = c.r * 4;
        mpoint.color.g = c.g;
        mpoint.color.b = c.b;
        mpoint.color.a = c.a;

        ma.markers.push_back(mpoint);
        return 0;
    }

    // 绘制路径信息
    int add_path_strip2d(int id,
                         Mat &path, double width, double height,
                         colorSimp c = colorSimp())
    {
        visualization_msgs::Marker mpath = get_default_marker(id);
        mpath.type = visualization_msgs::Marker::LINE_STRIP;
        mpath.color.r = c.r;
        mpath.color.g = c.g;
        mpath.color.b = c.b;
        mpath.color.a = c.a;
        mpath.points.clear();
        mpath.scale.x = width; // 设置线宽为 0.05

        geometry_msgs::Point pl;
        pl.z = height;
        for (int k = 0; k < path.rows(); ++k) {
            pl.x = path(k, 0);
            pl.y = path(k, 1);
            mpath.points.push_back(pl);
        }
        ma.markers.push_back(mpath);

        return 0;
    }

    // 绘制地图网格
    int add_scattered2d(int id,
                        Mat &points, double diameter, double height,
                        colorSimp c = colorSimp())
    {
        visualization_msgs::Marker mgrid = get_default_marker(id);
        mgrid.type = visualization_msgs::Marker::SPHERE_LIST;   // 表示一组球体
        mgrid.color.r = c.r;
        mgrid.color.g = c.g;
        mgrid.color.b = c.b;
        mgrid.color.a = c.a;
        mgrid.points.clear();
        mgrid.scale.x = mgrid.scale.y = mgrid.scale.z = diameter;

        geometry_msgs::Point pl;
        pl.z = height;
        for (int k = 0; k < points.rows(); ++k) {
            pl.x = points(k, 0);
            pl.y = points(k, 1);
            mgrid.points.push_back(pl);
        }
        ma.markers.push_back(mgrid);
        return 0;
    }

    int add_colored_path_strip2d(int id, 
                                 Mat& path, Vec& val, 
                                 double val_min, double width, double height, 
                                 colorSimp c_min = colorSimp(0.8, 0.2, 0.2, 1.0),
                                 colorSimp c_max = colorSimp(1.0, 1.0, 0.2, 1.0)) {
                                
        visualization_msgs::Marker m = get_default_marker(id);
        m.type = visualization_msgs::Marker::LINE_STRIP;

        return 0;
    }

    int add_arrow(int id,
                  double x0, double y0, double z0,
                  double x1, double y1, double z1,
                  double size,
                  colorSimp c = colorSimp(1.0, 0, 1.0, 1.0))
    {
        visualization_msgs::Marker m = get_default_marker(id);
        m.type = visualization_msgs::Marker::ARROW;
        m.color.r = c.r;
        m.color.g = c.g;
        m.color.b = c.b;
        m.color.a = c.a;
        m.scale.x = size * 0.5;
        m.scale.y = size;
        m.scale.z = size;
        geometry_msgs::Point pl;
        pl.x = x0;
        pl.y = y0;
        pl.z = z0;
        m.points.push_back(pl);
        pl.x = x1;
        pl.y = y1;
        pl.z = z1;
        m.points.push_back(pl);
        ma.markers.push_back(m);
        return 0;
    }

    int clear() {
        ma.markers.clear();
        id0 = 0;
        return 0;
    }


    int send() {
        if (ros::ok()) {
            pub.publish(ma);
        } 
        else {
            std::cout << "ros NOT ok." << std::endl;
        }
        return 0;
    }

};









#endif  // RVIZ_DIS_01




